#pragma once

#include "cartographer/rcm/common.h"
#include "cartographer/mapping/2d/xy_index.h"
#include "cartographer/mapping/2d/map_limits.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/grid_interface.h"
#include "cartographer/sensor/rangefinder_point.h"
#include "cartographer/rcm/dynamic/options.h"

namespace cartographer
{
    class DynPoint
    {
    public:
        using Ptr = std::shared_ptr<DynPoint>;

        DynPoint()
        {
        }

        int index = 0;
        int orgIndex = 0;
        bool motional = false;
        bool obsolete = false;
        Eigen::Array2i cellIdx;
        sensor::RangefinderPoint hit;
    };

    inline int GetLinearIndex(const Eigen::Array2i &cellIdx, const mapping::MapLimits &limits)
    {
        return cellIdx(0) + cellIdx(1) * limits.cell_limits().num_x_cells;
    }

    mapping::MapLimits GrowMapLimits(mapping::MapLimits &thisLimit, const Eigen::Vector2f &point);

    mapping::MapLimits GrowMapLimits(const sensor::RangeData &range_data, mapping::ProbabilityGrid *const grid);

    mapping::MapLimits ScaleGridResolution(const mapping::MapLimits &limits, const float pixelScale);

    class DynCloud
    {
    public:
        using Ptr = std::shared_ptr<DynCloud>;

        DynCloud()
        {
        }

        inline int Size()
        {
            return (int)points_.size();
        }

        double time_;

        std::vector<DynPoint::Ptr> points_;
    };

    class DynamicDetector
    {
    public:
        using Ptr = std::shared_ptr<DynamicDetector>;

        DynamicDetector()
        {
        }

        virtual ~DynamicDetector()
        {
        }

        virtual void DetectDynPoint(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time,
                                    std::vector<int> &cnts) = 0;
    };

    class SingleDynamicDetector : public DynamicDetector
    {
    public:
        using Ptr = std::shared_ptr<SingleDynamicDetector>;

        SingleDynamicDetector();

        void PrepareCloud(const common::Time time, int size);

        void DetectDynPoint(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time, std::vector<int> &cnts);

    private:
        std::map<int, int> hitCntMap_;

        std::deque<DynCloud::Ptr> cloudQueue_;
    };
} // namespace cartographer
